/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    StateEstimator.cpp
  * @author  Mentos Seetoo
  * @brief   
  * @note    THe following third-party libraries are used:
  *          - Eigen3
  * @warning 
  *     - At least C++11 is required.												 
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "Stste_Feedback/StateEstimator.h"

using namespace Quadruped;
/* Founctions ----------------------------------------------------------------*/
template <typename T>
StateEstimator<T>::StateEstimator()
{
    p_raw = Vec3<T>::Zero();
    v_raw = Vec3<T>::Zero();
    w_raw = Vec3<T>::Zero();
    rpy_raw = Vec3<T>::Zero();
    p = Vec3<T>::Zero();
    v = Vec3<T>::Zero();
    w = Vec3<T>::Zero();
    rpy = Vec3<T>::Zero();
    for(int leg(0); leg < 4; leg++)
    {
        p_foot_M[leg] = Vec3<T>::Zero();
        p_foot_G[leg] = Vec3<T>::Zero();
        p_initFoot[leg] = Vec3<T>::Zero();
    }
}





template class StateEstimator<float>;
template class StateEstimator<double>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/